#include <iostream>
#include <vector>
#include <queue>
#include <set>

struct Target
{
    int x;
    int y;
};
struct State
{
    int boxx;
    int boxy;
    int px;
    int py;
};
struct Action
{
    int type;//0 Move, 1Push
    int dx;
    int dy;
};
struct ActionState
{
    State state;
    Action action;
};
struct PathState
{
    State state;
    std::vector<Action> actions;
};
int getCost(const PathState &pathstate)
{
    int cost = 0;
    for(const auto & i : pathstate.actions)
    {
        cost += i.type;
    }
    return cost + pathstate.actions.size();
}
struct Compare {
    bool operator()(const PathState& a, const PathState& b) const {
        return getCost(a) > getCost(b);
    }
};
static int dxTab[] = {1 ,-1, 0,  0};
static int dyTab[] = {0 , 0, 1, -1};
class Solution {
public:
    void getMap(std::vector<std::vector<char>>& grid, State& info, Target& target)
    {
        for(int i = 0; i < grid.size(); ++i)
        {
            for(int j = 0; j < grid[i].size(); ++j)
            {
                if(grid[i][j] == 'T')
                {
                    grid[i][j] = '.';
                    target.x = j;
                    target.y = i;
                }
                if(grid[i][j] == 'B')
                {
                    grid[i][j] = '.';
                    info.boxx = j;
                    info.boxy = i;
                }
                if(grid[i][j] == 'S')
                {
                    grid[i][j] = '.';
                    info.px = j;
                    info.py = i;
                }
            }
        }
    }
    bool isAction(Action &action, const State &cur_state, std::vector<std::vector<char>>& grid)
    {
        int npx = cur_state.px + action.dx;
        int npy = cur_state.py + action.dy;
        int nboxx = cur_state.px + 2*action.dx;
        int nboxy = cur_state.py + 2*action.dy;
        if(npy < 0 || npy >= grid.size())
        {
            return false;
        }
        if(npx < 0 || npx >= grid[npy].size())
        {
            return false;
        }
        if(grid[npy][npx] == '#')
        {
            //下一步是墙
            return false;
        }
        if(grid[npy][npx] == '.'&& npx == cur_state.boxx && npy == cur_state.boxy)
        {
            //下一步是箱子
            action.type = 1;
            if(nboxy < 0 || nboxy >= grid.size())
            {
                return false;
            }
            if(nboxx < 0 || nboxx >= grid[nboxy].size())
            {
                return false;
            }
            if(grid[nboxy][nboxx] == '#')
            {
                return false;
            }
        }
        return true;
    }
    State move(const Action& action, const State &cur_state, std::vector<std::vector<char>>& grid)
    {
        State new_state = cur_state;
        new_state.px = cur_state.px + action.dx;
        new_state.py = cur_state.py + action.dy;
        if(action.type == 1)
        {
            new_state.boxx = cur_state.px + 2*action.dx;
            new_state.boxy = cur_state.py + 2*action.dy;
        }
        return new_state;
    }

    std::vector<ActionState> get_successor(const State &cur_state, std::vector<std::vector<char>>& grid)
    {
        std::vector<ActionState> ASList;
        for(int i = 0; i < 4; ++i)
        {
            ActionState actionState{};
            actionState.action.dx = dxTab[i];
            actionState.action.dy = dyTab[i];
            if(isAction(actionState.action, cur_state, grid))
            {
                actionState.state  = move(actionState.action, cur_state, grid);
                ASList.emplace_back(actionState);
            }
        }
        return ASList;
    }
    int minPushBox(std::vector<std::vector<char>>& grid) {
        State state{};
        Target target{};
        PathState pathState;
        getMap(grid, state, target);
        pathState.state = state;
        std::priority_queue<PathState, std::vector<PathState>, Compare> openlist;
        std::set<long> closelist;
        openlist.push(pathState);
        while(!openlist.empty())
        {
            PathState cur_state = openlist.top();
            openlist.pop();
            //std::cout<<"current state px: "<<cur_state.state.px<<","<<cur_state.state.py<<std::endl;
            //std::cout<<"current state box: "<<cur_state.state.boxx<<","<<cur_state.state.boxy<<std::endl;
            if(cur_state.state.boxx == target.x && cur_state.state.boxy == target.y)
            {
                std::cout<<"find target"<<std::endl;
                int count = 0;
                for(const auto& action : cur_state.actions)
                {
                    count += action.type;
                    std::cout<<"    action "<<action.dx<<","<<action.dy<<","<<action.type<<std::endl;
                }
                return count;
            }
            long hash = cur_state.state.boxx*1000 + cur_state.state.boxy*100 + cur_state.state.px*10 + cur_state.state.py;
            if(closelist.find(hash) != closelist.end())
            {
                //std::cout<<"find in closelist"<<std::endl;
                continue;
            }else{
                closelist.insert(hash);
            }
            auto succs = get_successor(cur_state.state, grid);
            for(const auto& succ : succs)
            {
                PathState next_state;
                next_state.state = move(succ.action, cur_state.state, grid);
                next_state.actions = cur_state.actions;
                next_state.actions.push_back(succ.action);
                //std::cout<<"    action "<<succ.action.dx<<","<<succ.action.dy<<","<<succ.action.type<<std::endl;
                //std::cout<<"    next state px: "<<next_state.state.px<<","<<next_state.state.py<<std::endl;
                //std::cout<<"    next state box: "<<next_state.state.boxx<<","<<next_state.state.boxy<<std::endl;
                openlist.push(next_state);
            }
        }
        return -1;
    }
};
int main() {
    std::vector<std::vector<char>> grid;
    //testcase1
//    grid.emplace_back(std::vector<char>{'#','#','#','#','#','#'});
//    grid.emplace_back(std::vector<char>{'#','T','#','#','#','#'});
//    grid.emplace_back(std::vector<char>{'#','.','.','B','.','#'});
//    grid.emplace_back(std::vector<char>{'#','.','#','#','.','#'});
//    grid.emplace_back(std::vector<char>{'#','.','.','.','S','#'});
//    grid.emplace_back(std::vector<char>{'#','#','#','#','#','#'});
     //testcase2
    grid.emplace_back(std::vector<char>{'.','.','#','.','.','.','.','#'});
    grid.emplace_back(std::vector<char>{'.','B','.','.','.','.','.','#'});
    grid.emplace_back(std::vector<char>{'.','.','S','.','.','.','.','.'});
    grid.emplace_back(std::vector<char>{'.','#','.','.','.','.','.','.'});
    grid.emplace_back(std::vector<char>{'.','.','.','.','.','.','.','.'});
    grid.emplace_back(std::vector<char>{'.','.','.','T','.','.','.','.'});
    grid.emplace_back(std::vector<char>{'.','.','.','.','.','.','.','#'});
    grid.emplace_back(std::vector<char>{'.','#','.','.','.','.','.','.'});
    // testcase3
//    grid.emplace_back(std::vector<char>{'#','.'});
//    grid.emplace_back(std::vector<char>{'B','S'});
//    grid.emplace_back(std::vector<char>{'T','#'});
    Solution solution;
    int st = solution.minPushBox(grid);
    std::cout << "Hello, World!" << st<< std::endl;
    return 0;
}
